/*******************************************************************************
Transmission type
00: 非循环同步；
01：循环同步；
FC：远程同步；
FD：远程异步：
FE：异步，制造商特定事件；
FF：异步，设备子协议特定事件
********************************************************************************/
//#define TRANS_EVERY_N_SYNC(n) (n) /*n = 1 to 240 */
//#define TRANS_SYNC_ACYCLIC    0    /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_SYNC_MIN        1    /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_SYNC_MAX        240  /* Trans after reception of n SYNC. n = 1 to 240 */
//#define TRANS_RTR_SYNC        252  /* Transmission on request */
//#define TRANS_RTR             253  /* Transmission on request */
//#define TRANS_EVENT_SPECIFIC  254  /* Transmission on event */
//#define TRANS_EVENT_PROFILE   255  /* Transmission on event */

#include "canopen_motor.h"
#include <rthw.h>
#include "main.h"
#include "stdlib.h"
#include "eeprom.h"
#include "leds.h"
#include "CallBack.h"
#include "io.h"
#include "zy_can.h"
#define DBG_TAG "canopen_motor"
#define DBG_LVL DBG_ERROR
#include <rtdbg.h>

#define motor_od    &Servo_OD_Data

#define CHECK_ID(id)            if(id > TOTAL_MOTORS || id < 1) { return IDERR;}
#define CHECK_STATUS(id)        if((canopenMotors[id - 1].statusWord.statusBit.stdStatus) != STATUS_OPERATION_EN) {return STATUSERR;}

#define INVALID_ID(id)          ((id) | 0x80000000)
#define VALID_ID(id)            ((id) & 0x7FFFFFFF)
#define ASK_MOTOR_TRY_TIMES     500
#define ASK_MOTOR_INTERVAL      100

#define TPDO_SYN_CYCLE          0x00000000              // 单位us
#define TPDO_TRANS_TYPE         TRANS_EVENT_SPECIFIC
#define TPDO_TRANS_EVENT_TIME   20                      // 伺服状态上报间隔，实测时间在15ms
#define FFB_TPDO_TRANS_EVENT_TIME   5                   // FFBWheel上传间隔
#define TPDO_STATUS_WORD_TIME   100                     // 状态字上报间隔


#define DICT_UINT8(i,s,d)       {.index = i, .subIndex = s, .dataType = uint8,  .count = 1, .data = d}
#define DICT_INT8(i,s,d)        {.index = i, .subIndex = s, .dataType = int8,   .count = 1, .data = d}
#define DICT_UINT16(i,s,d)      {.index = i, .subIndex = s, .dataType = uint16, .count = 2, .data = d}
#define DICT_INT16(i,s,d)       {.index = i, .subIndex = s, .dataType = int16,  .count = 2, .data = d}
#define DICT_UINT32(i,s,d)      {.index = i, .subIndex = s, .dataType = uint32, .count = 4, .data = d}
#define DICT_INT32(i,s,d)       {.index = i, .subIndex = s, .dataType = uint32, .count = 4, .data = d}

//字典映射 索引-子索引-数据长度
//无符号32位
#define MAP_INDEX(map)          ((map>>16)&0xFFFF)
#define MAP_SUBINDEX(map)       ((map>>8)&0xFF)
#define MAP_DATALEN(map)        ((map&0xFF)/8)


//无符号16位
#define MAP_TORQUE_PMAX_INRANGE 0x60E00010      // 行程内正力矩限幅(千分比)    这两个力阻碍方向盘运动，不管是打盘还是回盘
#define MAP_TORQUE_NMAX_INRANGE 0x60E10010      // 行程内负力矩限幅(千分比)

//有符号32位
#define MAP_ACTUAL_POS          0x60640020      // 实际位置(用户单位， 迈信0x20000 一圈,逆时针增大，顺时针减小，会减小到负值)
#define MAP_ACTUAL_VEL          0x606C0020      // 实际速度，数值有+-70左右的漂移
#define MAP_TARGET_POS          0x607A0020      // 目标位
#define MAP_MAX_PROFILE_VELOCITY    0x607F0020  // 最高转速限制
#define MAP_TARGET_VEL          0x60FF0020      // 目标速度

//有符号16位
#define MAP_ACTUAL_TORQUE       0x60770010      // 实际转矩 0.1% 正好是MAP_CUR_TORQUE_PER的0.1倍
#define MAP_TARGET_TORQUE       0x60710010      // 目标转矩

//无符号16位
#define MAP_CONTROL_WORD        0x60400010      // 控制字
#define MAP_STATUS_WORD         0x60410010      // 状态字
//无符号8位
#define MAP_MODE_OF_OPERATION   0x60600008      // 操作模式，在发送使能命令之前，需要确定此对象值，使能之后的运行模式以接收到使能命令时此对象值为准
#define MAP_MODE_OF_CURRENT     0x60610008      // 当前操作模式


#define MAP_MAX_PROFILE_VEL     0x607F0020      // 最高转速限制
#define MAP_PROFILE_VEL         0x60810020      // 运行速度 单位rpm
#define MAP_PROFILE_ACC         0x60830020      // 加速度  (单位：从 0rpm 加速到额定转速的加速时间 ms)
#define MAP_PROFILE_DEC         0x60840020      // 减速
#define MAP_HOMING_MATHOD       0x60980008      // 回零模式

#ifdef  STABLECU_FT2
#define GEAR_RATIO_INDEX        0x6093          // 实际是POSITON Factor
#else
#define GEAR_RATIO_INDEX        0x6091
#endif

#define DS402_CUSTOM(map,subindex)          \
        ((map-0x10000000)|(subindex << 8))      // 由于canfestivel中DS402变量只能定义一个，所以将DS402的变量映射到用户区，方便操作

static  uint8_t writeSdoRes;
static  uint16_t read16BitValue;                // 读取SDO的时候用于返回值
struct rt_mailbox mbSlaveId;
static uint32_t mbSlaveIdPool;


/**
 * @brief canopenMotors数组包含CANopen电机的配置信息。
 *
 * canopenMotors数组包含以下电机的配置信息：
 * - 电机1配置
 * - 电机2配置（如果存在多个电机）
 *
 * 每个配置信息包括节点ID和操作模式。可以根据需要添加更多电机的配置信息。
 */
canopen_motor_t canopenMotors[TOTAL_MOTORS] = {
    { .nodeId = CANOPEN_MOTOR_1_NODEID,.modesOfOperation = TQ_MODE},    // 此模式只是用来回零，回零完成之后会设置为TQ_MODE
#if TOTAL_MOTORS > 1
    { .nodeId = CANOPEN_MOTOR_2_NODEID,.modesOfOperation = PP_MODE},
#endif
#if TOTAL_MOTORS > 2
    { .nodeId = CANOPEN_MOTOR_3_NODEID,.modesOfOperation = PP_MODE},
#endif
};


/**
 * @brief localOdParam数组包含用于CANopen对象字典的本地参数设置。
 *
 * localOdParam数组包含以下对象字典条目：
 * - 服务端TPDO映射本地RPDO
 * - 本地TPDO映射服务器RPDO
 *
 * 每个条目都包括对象字典索引、子索引、数值以及数据大小。同时，还包括一些注释来解释各个条目的用途。
 */
static local_od_t localOdParam[] = 
{

    /* id 1 */
    /* 服务端TPDO映射本地RPDO */
    {.index = 0x1400, .subIndex = 0x01, .data = INVALID_ID(0x180+CANOPEN_MOTOR_1_NODEID), .size = 4},   // 无效RPDO1的cobid
    {.index = 0x1600, .subIndex = 0x00, .data = 2, .size = 1},                                          // RPDO1字典个数，与服务端TPDO一一对应
    {.index = 0x1600, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_ACTUAL_POS,1), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1600, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_ACTUAL_VEL,1), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1400, .subIndex = 0x01, .data = VALID_ID(0x180+CANOPEN_MOTOR_1_NODEID), .size = 4},     // 有效RPDO1的cobid

    {.index = 0x1401, .subIndex = 0x01, .data = INVALID_ID(0x280+CANOPEN_MOTOR_1_NODEID), .size = 4},   // 无效RPDO2的cobid
    {.index = 0x1601, .subIndex = 0x00, .data = 1, .size = 1},                                          // RPDO2字典个数，与服务端TPDO一一对应
    {.index = 0x1601, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_STATUS_WORD,1), .size = 4},            // RPDO2字典映射，与服务端TPDO一一对应
    {.index = 0x1401, .subIndex = 0x01, .data = VALID_ID(0x280+CANOPEN_MOTOR_1_NODEID), .size = 4},     // 有效RPDO2的cobid

    /* 本地TPDO映射服务器RPDO */
    {.index = 0x1800, .subIndex = 0x01, .data = INVALID_ID(0x200+CANOPEN_MOTOR_1_NODEID), .size = 4},   // 无效TPDO1的cobid
    {.index = 0x1A00, .subIndex = 0x00, .data = 0x02, .size = 1},
    {.index = 0x1A00, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_TARGET_TORQUE,1), .size = 4},
    {.index = 0x1A00, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_MAX_PROFILE_VEL,1), .size = 4},
    {.index = 0x1800, .subIndex = 0x01, .data = VALID_ID(0x200+CANOPEN_MOTOR_1_NODEID), .size = 4},     // 有效TPDO1的cobid


    {.index = 0x1801, .subIndex = 0x01, .data = INVALID_ID(0x300+CANOPEN_MOTOR_1_NODEID), .size = 4},   // 无效TPDO2的cobid
    {.index = 0x1A01, .subIndex = 0x00, .data = 0x01, .size = 1},
    {.index = 0x1A01, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_CONTROL_WORD,1), .size = 4},           // 映射控制字
    {.index = 0x1801, .subIndex = 0x01, .data = VALID_ID(0x300+CANOPEN_MOTOR_1_NODEID), .size = 4},     // 有效TPDO2的cobid

#if TOTAL_MOTORS > 1
    /* id 2 */
    {.index = 0x1402, .subIndex = 0x01, .data = INVALID_ID(0x180+CANOPEN_MOTOR_2_NODEID), .size = 4},   // 无效RPDO1的cobid
    {.index = 0x1602, .subIndex = 0x00, .data = 2, .size = 1},                                          // RPDO1字典个数，与服务端TPDO一一对应
    {.index = 0x1602, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_ACTUAL_POS,2), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1602, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_ACTUAL_VEL,2), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1402, .subIndex = 0x01, .data = VALID_ID(0x180+CANOPEN_MOTOR_2_NODEID), .size = 4},     // 有效RPDO1的cobid

    {.index = 0x1403, .subIndex = 0x01, .data = INVALID_ID(0x280+CANOPEN_MOTOR_2_NODEID), .size = 4},   // 无效RPDO2的cobid
    {.index = 0x1603, .subIndex = 0x00, .data = 1, .size = 1},                                          // RPDO2字典个数，与服务端TPDO一一对应
    {.index = 0x1603, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_STATUS_WORD,2), .size = 4},            // RPDO2字典映射，与服务端TPDO一一对应
    {.index = 0x1403, .subIndex = 0x01, .data = VALID_ID(0x280+CANOPEN_MOTOR_2_NODEID), .size = 4},     // 有效RPDO2的cobid

    /* 本地TPDO映射服务器RPDO */
    {.index = 0x1802, .subIndex = 0x01, .data = INVALID_ID(0x200+CANOPEN_MOTOR_2_NODEID), .size = 4},   // 无效TPDO1的cobid
    {.index = 0x1A02, .subIndex = 0x00, .data = 0x02, .size = 1},
    {.index = 0x1A02, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_TARGET_POS,2), .size = 4},
    {.index = 0x1A02, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_PROFILE_VEL,2), .size = 4},
    {.index = 0x1802, .subIndex = 0x01, .data = VALID_ID(0x200+CANOPEN_MOTOR_2_NODEID), .size = 4},     // 有效TPDO1的cobid

    {.index = 0x1803, .subIndex = 0x01, .data = INVALID_ID(0x300+CANOPEN_MOTOR_2_NODEID), .size = 4},   // 无效TPDO2的cobid
    {.index = 0x1A03, .subIndex = 0x00, .data = 0x01, .size = 1},
    {.index = 0x1A03, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_CONTROL_WORD,2), .size = 4},           //
    {.index = 0x1803, .subIndex = 0x01, .data = VALID_ID(0x300+CANOPEN_MOTOR_2_NODEID), .size = 4},     // 有效TPDO2的cobid
#endif

#if TOTAL_MOTORS > 2
    /* id 3 */
    {.index = 0x1404, .subIndex = 0x01, .data = INVALID_ID(0x180+CANOPEN_MOTOR_3_NODEID), .size = 4},   // 无效RPDO1的cobid
    {.index = 0x1604, .subIndex = 0x00, .data = 2, .size = 1},                                          // RPDO1字典个数，与服务端TPDO一一对应
    {.index = 0x1604, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_ACTUAL_POS,3), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1604, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_ACTUAL_VEL,3), .size = 4},             // RPDO1字典映射，与服务端TPDO一一对应
    {.index = 0x1404, .subIndex = 0x01, .data = VALID_ID(0x180+CANOPEN_MOTOR_3_NODEID), .size = 4},     // 有效RPDO1的cobid

    {.index = 0x1405, .subIndex = 0x01, .data = INVALID_ID(0x280+CANOPEN_MOTOR_3_NODEID), .size = 4},   // 无效RPDO2的cobid
    {.index = 0x1605, .subIndex = 0x00, .data = 1, .size = 1},                                          // RPDO2字典个数，与服务端TPDO一一对应
    {.index = 0x1605, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_STATUS_WORD,3), .size = 4},            // RPDO2字典映射，与服务端TPDO一一对应
    {.index = 0x1405, .subIndex = 0x01, .data = VALID_ID(0x280+CANOPEN_MOTOR_3_NODEID), .size = 4},     // 有效RPDO2的cobid

    /* 本地TPDO映射服务器RPDO */
    {.index = 0x1804, .subIndex = 0x01, .data = INVALID_ID(0x200+CANOPEN_MOTOR_3_NODEID), .size = 4},   // 无效TPDO1的cobid
    {.index = 0x1A04, .subIndex = 0x00, .data = 0x02, .size = 1},
    {.index = 0x1A04, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_TARGET_POS,3), .size = 4},
    {.index = 0x1A04, .subIndex = 0x02, .data = DS402_CUSTOM(MAP_PROFILE_VEL,3), .size = 4},
    {.index = 0x1804, .subIndex = 0x01, .data = VALID_ID(0x200+CANOPEN_MOTOR_3_NODEID), .size = 4},     // 有效TPDO1的cobid

    {.index = 0x1805, .subIndex = 0x01, .data = INVALID_ID(0x300+CANOPEN_MOTOR_3_NODEID), .size = 4},   // 无效TPDO2的cobid
    {.index = 0x1A05, .subIndex = 0x00, .data = 0x01, .size = 1},
    {.index = 0x1A05, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_CONTROL_WORD,3), .size = 4},           //
    {.index = 0x1805, .subIndex = 0x01, .data = VALID_ID(0x300+CANOPEN_MOTOR_3_NODEID), .size = 4},     // 有效TPDO2的cobid
#endif
};


///**
// * @brief localOdParamTQ数组包含用于CANopen对象字典的本地参数设置。
// *
// * localOdParam数组包含以下对象字典条目：
// * - 服务端TPDO映射本地RPDO
// * - 本地TPDO映射服务器RPDO
// *
// * 每个条目都包括对象字典索引、子索引、数值以及数据大小。同时，还包括一些注释来解释各个条目的用途。
// */
//static local_od_t localOdParamTQ[] =
//{
//    /* 本地TPDO映射服务器RPDO */
//    {.index = 0x1800, .subIndex = 0x01, .data = INVALID_ID(0x200+CANOPEN_MOTOR_1_NODEID), .size = 4},   // 无效TPDO1的cobid
//    {.index = 0x1A00, .subIndex = 0x00, .data = 0x01, .size = 1},
//    {.index = 0x1A00, .subIndex = 0x01, .data = DS402_CUSTOM(MAP_TARGET_TORQUE,1), .size = 4},          // 映射目标转矩
//    {.index = 0x1800, .subIndex = 0x01, .data = VALID_ID(0x200+CANOPEN_MOTOR_1_NODEID), .size = 4},     // 有效TPDO1的cobid
//
//};


/**
 * @brief 电机1 netOd1数组包含用于位置模式的CANopen对象字典条目。
 *
 * netOd1数组包含以下对象字典条目：
 * - 电子齿轮比设置
 * - 最大运行设置
 * - TPDO设置
 * - RPDO设置
 *
 * 每个条目都包括对象字典索引、子索引和相应的数值。同时，还包括一些注释来解释各个条目的用途。
 */
// 适用于力矩模式
static const net_sdo_od_t netOd1[] = {
    /* 控制模式设置 捷胜独有 */
    DICT_UINT8(0x2001, 0x01, 30),       //CANOpen控制模式
    DICT_UINT16(0x1017, 0x00, 0),
    /* 电子齿轮比设置 力反馈电机的编码器是17位，直接1：1兼容之前的逻辑 */
    DICT_UINT32(GEAR_RATIO_INDEX, 0x01,   FFB_ELECTRONIC_GEAR_NUMERATOR),
    DICT_UINT32(GEAR_RATIO_INDEX, 0x02,   FFB_ELECTRONIC_GEAR_DINOMINATOR),

    // 默认轮廓速设置
    DICT_UINT32(0x607F, 0x00,   FFB_SPEED_DEFAULT),
//    /* 加减速设置 */
//    DICT_UINT32(0x6083, 0x00,   MAX_MACHINE_RPM*1.0),
//    DICT_UINT32(0x6084, 0x00,   MAX_MACHINE_RPM*0.6),
    /* 限制最大运行速度 */

    /* TPDO设置 */
    DICT_UINT32(0x1800, 0x01,   INVALID_ID(0x180 + CANOPEN_MOTOR_1_NODEID)),    // 无效TPDO1的cobid
    DICT_UINT8( 0x1A00, 0x00,   0),                                             // 写TPDO1子索引个数为0
    DICT_UINT32(0x1A00, 0x01,   MAP_ACTUAL_POS),                                // 当前位置(用户单位)
    DICT_UINT32(0x1A00, 0x02,   MAP_ACTUAL_VEL),                                // 当前位置(用户单位)
    DICT_UINT8( 0x1A00, 0x00,   2),                                             // 写TPDO1子索引个数为1
    DICT_UINT8( 0x1800, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO1传输类型
    DICT_UINT16(0x1800, 0x05,   FFB_TPDO_TRANS_EVENT_TIME),                     // 写TPDO1发送周期
    DICT_UINT32(0x1800, 0x01,   VALID_ID(0x180 + CANOPEN_MOTOR_1_NODEID)),      // 有效效TPDO1的cobid

    DICT_UINT32(0x1801, 0x01,   INVALID_ID(0x280 + CANOPEN_MOTOR_1_NODEID)),    // 无效TPDO2 cobid
    DICT_UINT8( 0x1A01, 0x00,   0),                                             // 写TPDO2子索引个数为0
    DICT_UINT32(0x1A01, 0x01,   MAP_STATUS_WORD),                               // 状态字
    DICT_UINT8( 0x1A01, 0x00,   1),                                             // 写TPDO2子索引个数为1
    DICT_UINT8( 0x1801, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO2传输类型
    DICT_UINT16(0x1801, 0x05,   TPDO_STATUS_WORD_TIME),                         // 写TPDO2发送周期
    DICT_UINT32(0x1801, 0x01,   VALID_ID(0x280 + CANOPEN_MOTOR_1_NODEID)),      // 写TPDO2 cobid

    // 未使用的
    DICT_UINT32(0x1802, 0x01,   INVALID_ID(0x380 + CANOPEN_MOTOR_1_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A02, 0x00,   0),                                             // 写TPDO3子索引个数为0
    DICT_UINT32(0x1803, 0x01,   INVALID_ID(0x480 + CANOPEN_MOTOR_1_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A03, 0x00,   0),                                             // 写TPDO3子索引个数为0


    /* RPDO设置   */
    DICT_UINT32(0x1400, 0x01,   INVALID_ID(0x200 + CANOPEN_MOTOR_1_NODEID)),    // 无效RPDO1 cobid
    DICT_UINT8( 0x1600, 0x00,   0),                                             // 写RPDO1子索引个数为0
    DICT_INT32( 0x1600, 0x01,   MAP_TARGET_TORQUE),
    DICT_INT32( 0x1600, 0x02,   MAP_MAX_PROFILE_VEL),
    DICT_UINT8( 0x1600, 0x00,   2),                                             // 写RPDO1子索引个数为1
    DICT_UINT32(0x1400, 0x01,   VALID_ID(0x200 + CANOPEN_MOTOR_1_NODEID)),      // 写RPDO1 cobid

    DICT_UINT32(0x1401, 0x01,   INVALID_ID(0x300 + CANOPEN_MOTOR_1_NODEID)),    // 无效RPDO2 cobid
    DICT_UINT8( 0x1601, 0x00,   0),                                             // 写RPDO2子索引个数为0
    DICT_INT32( 0x1601,  0x01,   MAP_CONTROL_WORD),                             // 控制字
    DICT_UINT8( 0x1601, 0x00,   1),                                             // 写RPDO2子索引个数为1
    DICT_UINT32(0x1401, 0x01,   VALID_ID(0x300 + CANOPEN_MOTOR_1_NODEID))       // 写RPDO2 cobid
};

///**
// * @brief 电机1 netOd1TQ数组包含用于力矩模式的CANopen对象字典条目。
// *
// * netOd1数组包含以下对象字典条目：
// * - 电子齿轮比设置
// * - 最大运行设置
// * - TPDO设置
// * - RPDO设置
// *
// * 每个条目都包括对象字典索引、子索引和相应的数值。同时，还包括一些注释来解释各个条目的用途。
// */
//// 适用于力矩模式
//static const net_sdo_od_t netOd1TQ[] = {
//    // 最高速度限制
//    DICT_UINT32(0x607F, 0x00,   FFB_SPEED_MAX),    // 无效RPDO1 cobid
//    /* RPDO设置   */
//    DICT_UINT32(0x1400, 0x01,   INVALID_ID(0x200 + CANOPEN_MOTOR_1_NODEID)),    // 无效RPDO1 cobid
//    DICT_UINT8( 0x1600, 0x00,   0),                                             // 写RPDO1子索引个数为0
//    DICT_UINT32(0x1600, 0x01,   MAP_TARGET_TORQUE),                             // 目标转矩
//    DICT_UINT8( 0x1600, 0x00,   1),                                             // 写RPDO1子索引个数为1
//    DICT_UINT32(0x1400, 0x01,   VALID_ID(0x200 + CANOPEN_MOTOR_1_NODEID)),      // 写RPDO1 cobid
//
//};


/**
 * @brief 电机2 netOd2数组包含用于位置模式的CANopen对象字典条目。
 *
 * netOd2数组包含以下对象字典条目：
 * - 电子齿轮比设置
 * - 加减速设置
 * - TPDO设置
 * - RPDO设置
 *
 * 每个条目都包括对象字典索引、子索引和相应的数值。同时，还包括一些注释来解释各个条目的用途。
 */
// 适用于位置模式
static const net_sdo_od_t netOd2[] = {
    /* 控制模式设置 捷胜独有 */
    DICT_UINT8(0x2001, 0x01, 30),       //CANOpen控制模式
    DICT_UINT16(0x1017, 0x00, 0),
    /* 电子齿轮比设置 */
    DICT_UINT32(GEAR_RATIO_INDEX, 0x01,   ELECTRONIC_GEAR_NUMERATOR),
    DICT_UINT32(GEAR_RATIO_INDEX, 0x02,   ELECTRONIC_GEAR_DINOMINATOR),

    /* 加减速设置 */
    DICT_INT16( 0x6086, 0x00,   0x03),     // 0:无加减速；1：梯形；2：指数；3：梯形加指数
    DICT_UINT32(0x6083, 0x00,   80),
    DICT_UINT32(0x6084, 0x00,   80),

    /* TPDO设置 */
    DICT_UINT32(0x1800, 0x01,   INVALID_ID(0x180 + CANOPEN_MOTOR_2_NODEID)),    // 无效TPDO1的cobid
    DICT_UINT8( 0x1A00, 0x00,   0),                                             // 写TPDO1子索引个数为0
    DICT_UINT32(0x1A00, 0x01,   MAP_ACTUAL_POS),                                // 当前位置(用户单位)
    DICT_UINT32(0x1A00, 0x02,   MAP_ACTUAL_VEL),                                // 当前位置(用户单位)
    DICT_UINT8( 0x1A00, 0x00,   2),                                             // 写TPDO1子索引个数为1
    DICT_UINT8( 0x1800, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO1传输类型
    DICT_UINT16(0x1800, 0x05,   TPDO_TRANS_EVENT_TIME),                         // 写TPDO1发送周期
    DICT_UINT32(0x1800, 0x01,   VALID_ID(0x180 + CANOPEN_MOTOR_2_NODEID)),      // 有效效TPDO1的cobid

    DICT_UINT32(0x1801, 0x01,   INVALID_ID(0x280 + CANOPEN_MOTOR_2_NODEID)),    // 无效TPDO2 cobid
    DICT_UINT8( 0x1A01, 0x00,   0),                                             // 写TPDO2子索引个数为0
    DICT_UINT32(0x1A01, 0x01,   MAP_STATUS_WORD),                               // 状态字
    DICT_UINT8( 0x1A01, 0x00,   1),                                             // 写TPDO2子索引个数为1
    DICT_UINT8( 0x1801, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO2传输类型
    DICT_UINT16(0x1801, 0x05,   TPDO_STATUS_WORD_TIME),                         // 写TPDO2发送周期
    DICT_UINT32(0x1801, 0x01,   VALID_ID(0x280 + CANOPEN_MOTOR_2_NODEID)),      // 写TPDO2 cobid

    DICT_UINT32(0x1802, 0x01,   INVALID_ID(0x380 + CANOPEN_MOTOR_2_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A02, 0x00,   0),                                             // 写TPDO3子索引个数为0
    DICT_UINT32(0x1803, 0x01,   INVALID_ID(0x480 + CANOPEN_MOTOR_2_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A03, 0x00,   0),                                             // 写TPDO3子索引个数为0

    /* RPDO设置   */
    DICT_UINT32(0x1400, 0x01,   INVALID_ID(0x200 + CANOPEN_MOTOR_2_NODEID)),    // 无效RPDO1 cobid
    DICT_UINT8( 0x1600, 0x00,   0),                                             // 写RPDO1子索引个数为0
    DICT_INT32( 0x1600, 0x01,   MAP_TARGET_POS),                                // 目标位置
    DICT_INT32( 0x1600, 0x02,   MAP_PROFILE_VEL),                               // 目标转速
    DICT_UINT8( 0x1600, 0x00,   2),                                             // 写RPDO1子索引个数为1
    DICT_UINT32(0x1400, 0x01,   VALID_ID(0x200 + CANOPEN_MOTOR_2_NODEID)),      // 写RPDO1 cobid

    DICT_UINT32(0x1401, 0x01,   INVALID_ID(0x300 + CANOPEN_MOTOR_2_NODEID)),    // 无效RPDO2 cobid
    DICT_UINT8( 0x1601, 0x00,   0),                                             // 写RPDO2子索引个数为0
    DICT_UINT32(0x1601, 0x01,   MAP_CONTROL_WORD),                              // 控制字
    DICT_UINT8( 0x1601, 0x00,   1),                                             // 写RPDO2子索引个数为1
    DICT_UINT32(0x1401, 0x01,   VALID_ID(0x300 + CANOPEN_MOTOR_2_NODEID))       // 写RPDO2 cobid
};


/**
 * @brief 电机2 netOd2数组包含用于位置模式的CANopen对象字典条目。
 *
 * netOd2数组包含以下对象字典条目：
 * - 电子齿轮比设置
 * - 加减速设置
 * - TPDO设置
 * - RPDO设置
 *
 * 每个条目都包括对象字典索引、子索引和相应的数值。同时，还包括一些注释来解释各个条目的用途。
 */
// 适用于位置模式
static const net_sdo_od_t netOd3[] = {
    /* 控制模式设置 捷胜独有 */
    DICT_UINT8(0x2001, 0x01, 30),       //CANOpen控制模式
    DICT_UINT16(0x1017, 0x00, 0),
    /* 电子齿轮比设置 */
    DICT_UINT32(GEAR_RATIO_INDEX, 0x01,   ELECTRONIC_GEAR_NUMERATOR),
    DICT_UINT32(GEAR_RATIO_INDEX, 0x02,   ELECTRONIC_GEAR_DINOMINATOR),


    /* 加减速设置 */
    DICT_INT16( 0x6086, 0x00,   0x03),
    DICT_UINT32(0x6083, 0x00,   80),
    DICT_UINT32(0x6084, 0x00,   80),

    /* TPDO设置 */
    DICT_UINT32(0x1800, 0x01,   INVALID_ID(0x180 + CANOPEN_MOTOR_3_NODEID)),    // 无效TPDO1的cobid
    DICT_UINT8( 0x1A00, 0x00,   0),                                             // 写TPDO1子索引个数为0
    DICT_UINT32(0x1A00, 0x01,   MAP_ACTUAL_POS),                                // 当前位置(用户单位)
    DICT_UINT32(0x1A00, 0x02,   MAP_ACTUAL_VEL),                                // 当前位置(用户单位)
    DICT_UINT8( 0x1A00, 0x00,   2),                                             // 写TPDO1子索引个数为1
    DICT_UINT8( 0x1800, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO1传输类型
    DICT_UINT16(0x1800, 0x05,   TPDO_TRANS_EVENT_TIME),                         // 写TPDO1发送周期
    DICT_UINT32(0x1800, 0x01,   VALID_ID(0x180 + CANOPEN_MOTOR_3_NODEID)),      // 有效效TPDO1的cobid

    DICT_UINT32(0x1801, 0x01,   INVALID_ID(0x280 + CANOPEN_MOTOR_3_NODEID)),    // 无效TPDO2 cobid
    DICT_UINT8( 0x1A01, 0x00,   0),                                             // 写TPDO2子索引个数为0
    DICT_UINT32(0x1A01, 0x01,   MAP_STATUS_WORD),                               // 状态字
    DICT_UINT8( 0x1A01, 0x00,   1),                                             // 写TPDO2子索引个数为1
    DICT_UINT8( 0x1801, 0x02,   TPDO_TRANS_TYPE),                               // 写TPDO2传输类型
    DICT_UINT16(0x1801, 0x05,   TPDO_STATUS_WORD_TIME),                         // 写TPDO2发送周期
    DICT_UINT32(0x1801, 0x01,   VALID_ID(0x280 + CANOPEN_MOTOR_3_NODEID)),      // 写TPDO2 cobid

    DICT_UINT32(0x1802, 0x01,   INVALID_ID(0x380 + CANOPEN_MOTOR_3_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A02, 0x00,   0),                                             // 写TPDO3子索引个数为0
    DICT_UINT32(0x1803, 0x01,   INVALID_ID(0x480 + CANOPEN_MOTOR_3_NODEID)),    // 无效TPDO3 cobid
    DICT_UINT8( 0x1A03, 0x00,   0),                                             // 写TPDO3子索引个数为0

    /* RPDO设置   */
    DICT_UINT32(0x1400, 0x01,   INVALID_ID(0x200 + CANOPEN_MOTOR_3_NODEID)),    // 无效RPDO1 cobid
    DICT_UINT8( 0x1600, 0x00,   0),                                             // 写RPDO1子索引个数为0
    DICT_INT32( 0x1600, 0x01,   MAP_TARGET_POS),                                // 目标位置
    DICT_INT32( 0x1600, 0x02,   MAP_PROFILE_VEL),                               // 目标转速
    DICT_UINT8( 0x1600, 0x00,   2),                                             // 写RPDO1子索引个数为1
    DICT_UINT32(0x1400, 0x01,   VALID_ID(0x200 + CANOPEN_MOTOR_3_NODEID)),      // 写RPDO1 cobid

    DICT_UINT32(0x1401, 0x01,   INVALID_ID(0x300 + CANOPEN_MOTOR_3_NODEID)),    // 无效RPDO2 cobid
    DICT_UINT8( 0x1601, 0x00,   0),                                             // 写RPDO2子索引个数为0
    DICT_UINT32(0x1601, 0x01,   MAP_CONTROL_WORD),                              // 控制字
    DICT_UINT8( 0x1601, 0x00,   1),                                             // 写RPDO2子索引个数为1
    DICT_UINT32(0x1401, 0x01,   VALID_ID(0x300 + CANOPEN_MOTOR_3_NODEID))       // 写RPDO2 cobid
};

/**
 * @brief 重定向本地对象字典参数数组。
 *
 * 此数组用于指定要重定向到本地对象字典的参数。每个元素包含对象字典索引、子索引和新变量的指针。
 */
static const redirect_od_var_t redirectLocalOdParam[] = 
{
    {.index = 0x5040, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].controlWord},
    {.index = 0x5041, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].statusWord},
    {.index = 0x5060, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].modesOfOperation},
    {.index = 0x5061, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].modesOfCurrent },
    {.index = 0x5064, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].actualPos },
    {.index = 0x5081, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].profileVel },
    {.index = 0x506C, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].actualVel },
    {.index = 0x5071, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].targetTorque },
    {.index = 0x5077, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].actualTorque },
    {.index = 0x507A, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].targetPos },
    {.index = 0x507F, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].maxProfileVel },
    {.index = 0x50FF, .subIndex = 0x01, .pnewvar = (void*)&canopenMotors[0].targetVel },

#if TOTAL_MOTORS > 1
    {.index = 0x5040, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].controlWord},
    {.index = 0x5041, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].statusWord},
    {.index = 0x5060, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].modesOfOperation},
    {.index = 0x5061, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].modesOfCurrent },
    {.index = 0x5064, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].actualPos },
    {.index = 0x5081, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].profileVel },
    {.index = 0x506C, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].actualVel },
    {.index = 0x5071, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].targetTorque },
    {.index = 0x5077, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].actualTorque },
    {.index = 0x507A, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].targetPos },
    {.index = 0x507F, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].maxProfileVel },
    {.index = 0x50FF, .subIndex = 0x02, .pnewvar = (void*)&canopenMotors[1].targetVel },
#endif

#if TOTAL_MOTORS > 2
    {.index = 0x5040, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].controlWord},
    {.index = 0x5041, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].statusWord},
    {.index = 0x5060, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].modesOfOperation},
    {.index = 0x5061, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].modesOfCurrent },
    {.index = 0x5064, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].actualPos },
    {.index = 0x5081, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].profileVel },
    {.index = 0x506C, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].actualVel },
    {.index = 0x5071, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].targetTorque },
    {.index = 0x5077, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].actualTorque },
    {.index = 0x507A, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].targetPos },
    {.index = 0x507F, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].maxProfileVel },
    {.index = 0x50FF, .subIndex = 0x03, .pnewvar = (void*)&canopenMotors[2].targetVel },
#endif
};


/**
 * @brief 根据节点ID获取CANopen电机对象。
 *
 * 此函数根据给定的节点ID在CANopen电机数组中查找并返回匹配的电机对象。
 *
 * @param[in] nodeId 电机的节点ID。
 * @return 如果找到匹配的电机对象，则返回该对象的指针；否则返回NULL。
 */
canopen_motor_t* Canopen_Motor_Get(uint8_t nodeId)
{
    for(int i=0;i<TOTAL_MOTORS;++i)
    {
        if(canopenMotors[i].nodeId == nodeId)
        {
            return &canopenMotors[i];
        }
    }
    return NULL;
}

static motor_err_t Motor_Std_Control(uint8_t nodeId,uint16_t cw);

/**
 * 写SDO的回调函数
 *
 *
 */

/**
 * @brief 写入网络对象字典 SDO 的回调函数。
 *
 * 此函数用于处理写入网络对象字典的回调操作。它检查写入操作的结果并根据需要进行处理。
 *
 * @param[in] d CO_Data结构的指针，表示CANopen网络数据。
 * @param[in] nodeId 电机的节点ID。
 */
static void WriteNetSdoCb(CO_Data* d, uint8_t nodeId)
{
    uint32_t abortCode;
    writeSdoRes = 0xFF;
    writeSdoRes = getWriteResultNetworkDict(d, nodeId, &abortCode);//获取写入网络字典的结果

    // 检查写入结果是否完成
    if(writeSdoRes != SDO_FINISHED)
    {
        LOG_E("sdo reply error,node id = %d,abortCode = %x \r\n",nodeId,abortCode);
        return;
    }

    // 关闭SDO传输
    closeSDOtransfer(d, nodeId, SDO_CLIENT);
    if(nodeId <= TOTAL_MOTORS && nodeId >=1)
    {
        rt_sem_release(canopenMotors[nodeId - 1].semSdoReply);
    }

    return;

}

/**
 * 读SDO的回调函数
 */
static void ReadNetSdoCb(CO_Data* d, uint8_t nodeId)
{
    uint32_t abortCode;
    uint32_t rcvData,rcvSize;
    uint16_t index;
//    uint8_t subIndex;
    uint8_t readSdoRes;
    readSdoRes = 0xFF;

    //对字典进行赋值,目前只有一个sdoline
    index = d->transfers[0].index;
//    subIndex = d->transfers[0].subIndex;
    if((index & 0xF000) == 0x2000)  // 伺服的参数设置SDO
    {
        rcvSize = 2;
        readSdoRes = getReadResultNetworkDict(d, nodeId, &rcvData,&rcvSize,&abortCode);
        read16BitValue = rcvData;
    }
//  switch(index)
//  {
//      case 0x1010:
//      {
//          switch(subIndex)
//          {
//              case 0x01:
//              rcvSize = 4;
//              readSdoRes = getReadResultNetworkDict(d, nodeId, &rcvData,&rcvSize,&abortCode);
////                *maxSineServoData.pSaveResult = rcvData;
//          }
//      }
//      break;
//
//      default :
//
//      break;
//  }

    if(readSdoRes != SDO_FINISHED)
    {
        LOG_E("sdo reply error,node id = %d,abortCode = %x \r\n",nodeId,abortCode);
        closeSDOtransfer(d, nodeId, SDO_CLIENT);
        return;
    }

    if(nodeId <= TOTAL_MOTORS && nodeId >=1)
    {
        rt_sem_release(canopenMotors[nodeId - 1].semSdoReply);
    }

    return;

}

/**
 * 将SDO发送到总线上，设置回调函数，在50ms内没有收到反馈则失败
 *
 * @param pParam 要配置的sdo参数
 *
 */
static uint8_t Motor_Write_NetSdo(struct net_sdo_prameter* pParam,canopen_motor_t* pMotor)
{
    struct net_sdo_prameter* p = pParam;
    rt_err_t result;
    writeNetworkDictCallBack(p->d,
                             p->targetID,
                             p->od.index,
                             p->od.subIndex,
                             p->od.count,
                             p->od.dataType,
                             p->pdata,
                             p->callback,0);

    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    if(result == -RT_ETIMEOUT)  //超时
    {
        LOG_E("sdo no reply,node id = %d",p->targetID);
    }
    return writeSdoRes;
}

/**
 * 将SDO发送到总线上，最多尝试n次，如果没有成功，则返回错误码
 *
 * @param pParam 要配置的sdo参数
 * @param n 尝试的次数

 */
static uint8_t Motor_Write_NetSdo_Until(struct net_sdo_prameter* pParam,canopen_motor_t* pMotor,uint32_t n)
{
    uint8_t ret=0xFF;
    uint32_t count = n;
    while(--count > 0)
    {
        if(Motor_Write_NetSdo(pParam,pMotor) == SDO_FINISHED)
        {
            ret = 0;
            break;
        }
    }
    return ret;
}


/*
 * 设置目标力矩
*/
static motor_err_t Motor_Set_TargetTorque(uint8_t nodeId,int16_t torque,uint32_t maxSpeed)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    if(canopenMotors[nodeId - 1].modesOfOperation != TQ_MODE)
    {
        return MODEERR;
    }
    canopenMotors[nodeId - 1].targetTorque  = torque;
    canopenMotors[nodeId - 1].maxProfileVel  = maxSpeed;
    sendOnePDOevent(motor_od,(nodeId - 1)*MAX_RPDO_NUM_PER_NODE);

    return NOERR;


}

/*
 * 设置目标位置和速度
*/
static motor_err_t Motor_Set_TargetPos(uint8_t nodeId,int32_t pos,uint32_t vel)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    if(canopenMotors[nodeId - 1].modesOfOperation != PP_MODE)
    {
        return MODEERR;
    }

    canopenMotors[nodeId - 1].targetPos = pos;
    canopenMotors[nodeId - 1].profileVel = vel;
    sendOnePDOevent(motor_od,(nodeId - 1)*MAX_RPDO_NUM_PER_NODE);
    rt_thread_delay(1);
    canopenMotors[nodeId - 1].controlWord = 0x2F;       // 清零 new_set_point bit
    sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE + 1);
    rt_thread_delay(2);
    canopenMotors[nodeId - 1].controlWord = 0x3F;       // 设置 new_set_point bit
    sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE + 1);

    return NOERR;
}

/*
 * 设置目标速度
*/
static motor_err_t Motor_Set_TargetVel(uint8_t nodeId,int32_t  vel)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    if(canopenMotors[nodeId - 1].modesOfOperation != PV_MODE)
    {
        return MODEERR;
    }
    canopenMotors[nodeId - 1].targetVel = vel;
    sendOnePDOevent(motor_od,(nodeId - 1)*MAX_RPDO_NUM_PER_NODE);
    return NOERR;
}

/*
 * 获取目标力矩
*/
static motor_err_t Motor_Get_Torque(uint8_t nodeId,int16_t* pTorque)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);

    *pTorque = canopenMotors[nodeId - 1].actualTorque;
    return NOERR;

}

/*
 * 获取目标位置
*/
static motor_err_t Motor_Get_Pos(uint8_t nodeId,int32_t* pPos)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    *pPos = canopenMotors[nodeId - 1].actualPos;
    return NOERR;
}

/*
 * 获取目标速度
*/
static motor_err_t Motor_Get_Vel(uint8_t nodeId,int32_t* pVel)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    *pVel = canopenMotors[nodeId - 1].actualVel;
    return NOERR;
}

/*
 * 获取电机状态字
*/
static motor_err_t Motor_Get_MotorStatus(uint8_t nodeId,uint16_t* pStatusWord)
{
    CHECK_ID(nodeId);
    *pStatusWord = canopenMotors[nodeId - 1].statusWord.statusWord;
    return NOERR;
}

static motor_err_t Motor_Quick_Stop(uint8_t nodeId)
{
    CHECK_ID(nodeId);
    canopenMotors[nodeId - 1].controlWord = CONTROL_TO_QUICKSTOP;

    Motor_Std_Control(nodeId,CONTROL_TO_QUICKSTOP);
    return NOERR;

}

/*
 * 电机控制字操作
*/
static motor_err_t Motor_Std_Control(uint8_t nodeId,uint16_t cw)
{
    CHECK_ID(nodeId);
    canopenMotors[nodeId - 1].controlWord = cw;
    sendOnePDOevent(motor_od,(nodeId - 1)*MAX_RPDO_NUM_PER_NODE+1);
    return NOERR;
}

/*
 * 设置电机操作模式
*/
static rt_err_t Motor_Set_OperationMode(uint8_t nodeId,uint8_t op)
{
    rt_err_t result;
    uint8_t data = op;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    writeNetworkDictCallBack(motor_od,
                            nodeId,
                            MAP_INDEX(MAP_MODE_OF_OPERATION),
                            MAP_SUBINDEX(MAP_MODE_OF_OPERATION),
                            MAP_DATALEN(MAP_MODE_OF_OPERATION),
                            uint8,
                            &data,
                            WriteNetSdoCb,
                            0);
    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    if(result == RT_EOK){       // 成功更改模式
        pMotor->modesOfOperation = op;
    }
    return result;
}

/*
 * 设置轮廓速度
*/
static rt_err_t Motor_Set_ProfileVel(uint8_t nodeId,uint32_t vel)
{
    rt_err_t result;
    uint32_t data = vel;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    writeNetworkDictCallBack(motor_od,
                            nodeId,
                            MAP_INDEX(MAP_PROFILE_VEL),
                            MAP_SUBINDEX(MAP_PROFILE_VEL),
                            MAP_DATALEN(MAP_PROFILE_VEL),
                            uint32,
                            &data,
                            WriteNetSdoCb,
                            0);
    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    return result;
}

/*
 * 设置轮廓加速度
*/
static rt_err_t Motor_Set_ProfileAcc(uint8_t nodeId,uint32_t acc)
{
    rt_err_t result;
    uint32_t data = acc;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    writeNetworkDictCallBack(motor_od,
                            nodeId,
                            MAP_INDEX(MAP_PROFILE_ACC),
                            MAP_SUBINDEX(MAP_PROFILE_ACC),
                            MAP_DATALEN(MAP_PROFILE_ACC),
                            uint32,
                            &data,
                            WriteNetSdoCb,
                            0);
    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    return result;
}

/*
 * 设置轮廓减速度
*/
static rt_err_t Motor_Set_ProfileDec(uint8_t nodeId,uint32_t dec)
{
    rt_err_t result;
    uint32_t data = dec;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    writeNetworkDictCallBack(motor_od,
                            nodeId,
                            MAP_INDEX(MAP_PROFILE_DEC),
                            MAP_SUBINDEX(MAP_PROFILE_DEC),
                            MAP_DATALEN(MAP_PROFILE_DEC),
                            uint32,
                            &data,
                            WriteNetSdoCb,
                            0);
    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    return result;
}

/*
 * 设置轮廓减速度
*/
static rt_err_t Motor_Set_HomingMethod(uint8_t nodeId,int8_t method)
{
    rt_err_t result;
    uint32_t data = method;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    writeNetworkDictCallBack(motor_od,
                            nodeId,
                            MAP_INDEX(MAP_HOMING_MATHOD),
                            MAP_SUBINDEX(MAP_HOMING_MATHOD),
                            MAP_DATALEN(MAP_HOMING_MATHOD),
                            int8,
                            &data,
                            WriteNetSdoCb,
                            0);
    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    return result;
}

static int Motor_Struct_Init(canopen_motor_t* pMotor,uint32_t count)
{
    char semName[16] = {0};
    if(pMotor == NULL)
    {
        return -1;
    }
    // 用到的信号量初始化
    for(int i=0; i<count; ++i)
    {
        memset(semName,0,sizeof(semName));
        sprintf(semName,"sdo %d",i);
        pMotor[i].semSdoReply = rt_sem_create(semName,0,RT_IPC_FLAG_FIFO);
        if(pMotor[i].semSdoReply == NULL)
        {
            return 1;
        }
    }

    // 将字典重定向到结构体
    for(int i=0;i<sizeof(redirectLocalOdParam)/sizeof(redirect_od_var_t);i++)
    {
        redirectODvar(motor_od,
                    redirectLocalOdParam[i].index,
                    redirectLocalOdParam[i].subIndex,
                    redirectLocalOdParam[i].pnewvar
                    );
    }
    return 0;
}

/*
 * 复位从节点
*/
static int Reset_Slave(canopen_motor_t* pMotor,int tryCount)
{
    uint8_t* idfromSlave = RT_NULL;
    int i;
    for(i=0;i<tryCount;++i) //上电复位电机
    {
        masterSendNMTstateChange(motor_od,pMotor->nodeId,NMT_Reset_Node);
        if(rt_mb_recv(&mbSlaveId,(rt_ubase_t*)&idfromSlave,ASK_MOTOR_INTERVAL) == RT_EOK)
        {
            if(*idfromSlave == pMotor->nodeId)
            {
                break;
            }
            else
            {
                LOG_D("invalid id recived");
            }

        }
    }
    if(i==ASK_MOTOR_TRY_TIMES)
    {
        LOG_E("no servo detected!\r\n");
        return -1;
    }
    return 0;
}

/*
 * 配置网络字典
 * 
*/
static void Config_Net_Od(CO_Data* d,const net_sdo_od_t* netOd,int odSize,canopen_motor_t* pMotor)
{
    struct net_sdo_prameter parm;
    int i=0;
    parm.d = d;
    parm.callback = WriteNetSdoCb;
    parm.targetID = pMotor->nodeId;

    for(i = 0; i < odSize; i++)
    {
        parm.od.index = netOd[i].index;
        parm.od.subIndex = netOd[i].subIndex;
        parm.od.count = netOd[i].count;
        parm.od.dataType = netOd[i].dataType;
        parm.pdata = (void*)&netOd[i].data;
        //配置出错，直接return
        if(Motor_Write_NetSdo_Until(&parm,pMotor,2))
        {
            LOG_E("Config_canopen_od failed,index = %x;subindex = %x!\r\n",netOd[i].index,netOd[i].subIndex);
            return;
        }
        //延时5ms配置下一个
        rt_thread_delay(5);
    }

    return;
}




/**
 * @brief CANopen电机线程入口函数。
 *
 * 此函数作为CANopen电机线程的入口点，负责初始化和管理电机控制。
 *
 * @param[in] parameter 线程参数（未使用）。
 */
extern rt_sem_t sem_motorReady[];
uint8_t motorInited[TOTAL_MOTORS] = {0};
void Canopen_Motor_Thread_Entry(void* parameter)
{
    rt_err_t result;
    int i;
    result = rt_mb_init(&mbSlaveId,
                        "mbSlaveId",
                        &mbSlaveIdPool,
                        sizeof(mbSlaveIdPool)/4,
                        RT_IPC_FLAG_FIFO);
    if(result != RT_EOK)
    {
        LOG_E("mb init failed!\r\n");
        return;
    }

    if(Motor_Struct_Init(canopenMotors,TOTAL_MOTORS))
    {
        LOG_E("motor struct init failed!\r\n");
        return;
    }
    CANOpen_RegisterCB();
    // 1. 设置自身状态,并发送指令检测伺服是否在线
    setNodeId(motor_od,MASTER_NODE_ID);
    setState(motor_od,Pre_operational );
    // 打开主电接触器
//    Dout_Write_Channel(1, 1);
//    rt_thread_delay(1000);
    // 复位伺服
    for(int i=0;i<TOTAL_MOTORS;i++){
        if(Reset_Slave(&canopenMotors[i],ASK_MOTOR_TRY_TIMES))
        {
            LOG_E("motor id %d reset failed!\r\n",i);
            return;
        }
    }

    // 2.设置自身字典数据，为后面周期获取slave数据做准备
    for(i=0;i<sizeof(localOdParam)/sizeof(local_od_t);i++)
    {
        writeLocalDict(motor_od, localOdParam[i].index, localOdParam[i].subIndex, &localOdParam[i].data, &localOdParam[i].size, 0);
    }

    // 3.并设置servo的OD
    Config_Net_Od(motor_od,netOd1,sizeof(netOd1)/sizeof(netOd1[0]),&canopenMotors[0]);
    // 4.设置操作模式
    Motor_Set_OperationMode(canopenMotors[0].nodeId,canopenMotors[0].modesOfOperation);
#if TOTAL_MOTORS > 1
    Config_Net_Od(motor_od,netOd2,sizeof(netOd2)/sizeof(netOd2[0]),&canopenMotors[1]);
    Motor_Set_OperationMode(canopenMotors[1].nodeId,canopenMotors[1].modesOfOperation);
#endif

#if TOTAL_MOTORS > 2
    Config_Net_Od(motor_od,netOd3,sizeof(netOd3)/sizeof(netOd3[0]),&canopenMotors[2]);
    Motor_Set_OperationMode(canopenMotors[2].nodeId,canopenMotors[2].modesOfOperation);
#endif


    // 5.设置自身为操作模式
    setState(motor_od,Operational);

    // 6.启动所有从节点开始通讯
    rt_thread_delay(2000);
    masterSendNMTstateChange(motor_od, 1, NMT_Start_Node);
    rt_thread_delay(1000);
    masterSendNMTstateChange(motor_od, 2, NMT_Start_Node);
    rt_thread_delay(1000);
    masterSendNMTstateChange(motor_od, 3, NMT_Start_Node);

    while(1)
    {
        for(i = 0; i< TOTAL_MOTORS; ++i)
        {
            rt_thread_delay(200);
            switch(canopenMotors[i].statusWord.statusBit.stdStatus)
            {
                case STATUS_NOT_READY:
                case STATUS_NOT_READY_QUICK_STOP:
                    Motor_Std_Control(canopenMotors[i].nodeId,CONTROL_TO_READY);
                    break;

                case STATUS_READY:
                    if(canopenMotors[i].controlWord != CONTROL_TO_QUICKSTOP)
                    {
                        Motor_Std_Control(canopenMotors[i].nodeId,CONTROL_TO_SWITCHED_ON);
                    }
                    break;

                case STATUS_SWITCHED_ON:
                    Motor_Std_Control(canopenMotors[i].nodeId,CONTROL_TO_OPERATION_EN);
                    break;

                case STATUS_OPERATION_EN:
                    if( !motorInited[i] && canopenMotors[i].statusWord.statusBit.stdStatus == STATUS_OPERATION_EN)
                    {
                        motorInited[i] = 1;
                        rt_sem_release(sem_motorReady[i]);
                    }
                    break;

                case STATUS_QUICK_STOP:

                    break;
            }
        }
    }
}


motor_err_t Canopen_Motor_Set_OperationMode(uint8_t nodeId,uint8_t mode)
{
    return Motor_Set_OperationMode(nodeId,mode);
}

/*
 * @回零相关接口函数
 */

/*
 * @brief 调用回零模式，将当前位置置为电机的零点
 */
void Canopen_Motor_Set_Zero(uint8_t nodeId,uint8_t runMode)
{
    rt_err_t ret;
    uint32_t tryTime = 0;
    canopen_motor_t* pcanopenMotor = Canopen_Motor_Get(nodeId);
    if(pcanopenMotor != NULL)
    {
        // 回零方式35，以当前位置为机械原点，触发原点回零后（6040控制字0F-->1F)，用当前位置6064=607C
        Motor_Set_OperationMode(nodeId,HM_MODE);
        ret = Motor_Set_HomingMethod(nodeId,35);
        rt_thread_delay(50);
        if(ret == RT_EOK)
        {
            pcanopenMotor->controlWord = 0x0F;
            sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE+1);
            rt_thread_delay(5);
            pcanopenMotor->controlWord = 0x1F;
            sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE+1);
        }
        rt_thread_delay(50);
        pcanopenMotor->homePul = pcanopenMotor->actualPos;
        ret = Motor_Set_OperationMode(nodeId,runMode);
        while(ret != RT_EOK && tryTime < 10){
            tryTime++;
            ret = Motor_Set_OperationMode(nodeId,runMode);
        }
        if(ret == RT_EOK){
            LOG_D("nodeid:%d;new mode:%d",nodeId,runMode);
        }

    }

}

/*
 *  @brief  设置当前位置为home pulse
 */
void Canopen_Motor_Set_HomePul(uint8_t nodeId)
{
    canopen_motor_t* pcanopenMotor = Canopen_Motor_Get(nodeId);
    if(pcanopenMotor != NULL)
    {
        pcanopenMotor->homePul = pcanopenMotor->actualPos;
    }

}

motor_err_t Canopen_Motor_Set_TargetTorque(uint8_t nodeId,int16_t torque,uint32_t maxSpeed)
{
    return Motor_Set_TargetTorque(nodeId, torque,maxSpeed);
}

/*
 * @brief  设置目标位置和速度
 */
motor_err_t Canopen_Motor_Set_TargetPos(uint8_t nodeId,int32_t pos,uint32_t vel)
{
    CHECK_ID(nodeId);
    CHECK_STATUS(nodeId);
    if(canopenMotors[nodeId - 1].modesOfOperation != PP_MODE)
    {
        return MODEERR;
    }

    canopenMotors[nodeId - 1].targetPos = pos;
    canopenMotors[nodeId - 1].profileVel = vel;
    sendOnePDOevent(motor_od,(nodeId - 1)*MAX_RPDO_NUM_PER_NODE );
    rt_thread_delay(1);
    canopenMotors[nodeId - 1].controlWord = 0x2F;       // 清零 new_set_point bit
    sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE+1);
    rt_thread_delay(2);
    canopenMotors[nodeId - 1].controlWord = 0x3F;       // 设置 new_set_point bit
    sendOnePDOevent(motor_od,(nodeId -1)*MAX_RPDO_NUM_PER_NODE+1);

    return NOERR;
}

int Canopen_Motor_Get_Pos(uint8_t nodeId)
{
    int retPos = 0;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    if(pMotor)
    {
        pMotor->curPul = pMotor->actualPos - pMotor->homePul;
        retPos = pMotor->curPul;
    }
    return retPos;
}

// 将电机的脉冲换算成0-360°的范围，单位是0.1°。前提是电子齿轮比的设置1个脉冲正好是0.1°
int Canopen_Motor_Get_Angle(uint8_t nodeId)
{
    int n = 0,angle = 0;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    if(pMotor)
    {
        pMotor->curPul = pMotor->actualPos - pMotor->homePul;
        // 修正homePul
        n = pMotor->curPul/3600;
        if(n != 0)
        {
            pMotor->homePul += n*3600;
            pMotor->curPul = pMotor->actualPos - pMotor->homePul;
        }
        // 此时pMotor->curPul在-3600~3600的范围内
        angle = pMotor->curPul>=0 ? pMotor->curPul:pMotor->curPul+3600;
    }
    return angle;
}


int Canopen_Motor_Get_Vel(uint8_t nodeId)
{
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    if(pMotor)
    {
        return pMotor->actualVel;
    }
    return 0;
}

void Motor_Home_Rcv(uint32_t evt);


void Canopen_Motor_Home_Start(uint8_t nodeId)
{

    for(int i=0;i<TOTAL_MOTORS;++i)
    {
        if(canopenMotors[i].nodeId == nodeId)
        {
            if(canopenMotors[i].homeStatus < BHS_START)
            {
                canopenMotors[i].homeStatus = BHS_START;
                Motor_Home_Rcv(nodeId);
            }
            return;
        }
    }
}

/*
 *  @brief 控制电机以vel的速度，走相对于当前位置的plus的脉冲
 *  @parameter
 *  nodeId:电机id
 *  plus:相对脉冲数
 *  vel:运动的速度
 *
 */
void Canopen_Motor_Home_Move(uint8_t nodeId,int32_t plus,uint32_t vel)
{
    for(int i=0;i<TOTAL_MOTORS;++i)
    {
        if(canopenMotors[i].nodeId == nodeId)
        {
            if(canopenMotors[i].homeStatus >= BHS_START)
            {
                canopenMotors[i].homeStatus = BHS_MOVE;
                int level = rt_hw_interrupt_disable();
                int pulse = canopenMotors[i].actualPos + plus;
                rt_hw_interrupt_enable(level);
                Motor_Set_TargetPos(nodeId,pulse,vel);
            }
            return;
        }
    }
}

/*
 *  @brief设置电机的home状态，纯粹的设置结构体的状态，不执行任何操作，回零的时候使用
 *  @parameter
 *  nodeId:电机id
 *  status:要设置的回零状态
 *
 */
motor_home_status_t Canopen_Motor_HomeStatus_Set(uint8_t nodeId,uint8_t status)
{
    canopen_motor_t* pcanopenMotor = Canopen_Motor_Get(nodeId);
    if(pcanopenMotor != NULL)
    {
        pcanopenMotor->homeStatus = status;
    }
    return status;
}

motor_home_status_t Canopen_Motor_HomeStatus_Get(uint8_t nodeId)
{
    canopen_motor_t* pcanopenMotor = Canopen_Motor_Get(nodeId);
    if(pcanopenMotor != NULL)
    {
        return pcanopenMotor->homeStatus;
    }
    else
    {
        return BHS_IDLE;
    }
}


/* 伺服参数设置API */
rt_err_t Canopen_Motor_WriteSdo_16Bit(uint8_t nodeId,uint16_t index,uint8_t subIndex,uint16_t data)
{
    rt_err_t result;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    net_sdo_od_t writeOd = DICT_UINT16(nodeId,index,subIndex);
    writeNetworkDictCallBack(motor_od,
                             nodeId,
                             writeOd.index,
                             writeOd.subIndex,
                             writeOd.count,
                             writeOd.dataType,
                             &writeOd.data,
                             WriteNetSdoCb,0);

    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    return result;
}

rt_err_t Canopen_Motor_ReadSdo_16Bit(uint8_t nodeId,uint16_t index,uint8_t subIndex,uint16_t* data)
{
    rt_err_t result;
    canopen_motor_t* pMotor = Canopen_Motor_Get(nodeId);
    net_sdo_od_t readOd = DICT_UINT16(nodeId,index,subIndex);
    readNetworkDictCallback(motor_od,
                             nodeId,
                             readOd.index,
                             readOd.subIndex,
                             readOd.dataType,
                             ReadNetSdoCb,0);

    /* 等待从邮箱中收取邮件 */
    result = rt_sem_take(pMotor->semSdoReply,50);
    if(result == RT_EOK)
    {
        *data = read16BitValue;
    }
    return result;
}

void Canopen_Motor_Stop_Node(void)
{
    for(int i=0;i<TOTAL_MOTORS;i++)
    {
        Motor_Std_Control(canopenMotors[i].nodeId,CONTROL_TO_QUICKSTOP);
        masterSendNMTstateChange(motor_od, i+1, NMT_Reset_Node);
        rt_thread_delay(20);
    }
    HAL_CAN_DeInit(CANOPEN_CAN_HANDLE);
}

///*
// *  @brief 将电机的操作模式设置为力矩模式。需配合字典。
// *  @parameter
// *  nodeId:电机id
// *
// */
//void Canopen_Config_TQ_OD(uint8_t nodeId)
//{
//    for(int i=0;i<sizeof(localOdParamTQ)/sizeof(local_od_t);i++)
//    {
//        writeLocalDict(motor_od, localOdParamTQ[i].index, localOdParamTQ[i].subIndex, &localOdParamTQ[i].data, &localOdParamTQ[i].size, 0);
//    }
//
//    Config_Net_Od(motor_od,netOd1TQ,sizeof(netOd1TQ)/sizeof(netOd1TQ[0]),&canopenMotors[0]);
//
//}

